#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_eigen/tf2_eigen.h>

using CloudRGB = pcl::PointCloud<pcl::PointXYZRGB>;
using Policy = message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2>;

class TfMerger {
 public:
  TfMerger(ros::NodeHandle& nh, ros::NodeHandle& pnh) : tf_listener_(tf_buffer_) {
    pnh.param<std::string>("topic_a", topic_a_, "/rx_cloud_A");
    pnh.param<std::string>("topic_c", topic_c_, "/rx_cloud_C");
    pnh.param<std::string>("out_topic", out_topic_, "/merged_cloud");
    pnh.param<std::string>("fixed_frame", fixed_frame_, "map");
    double slop{0.05}; pnh.param("sync_slop", slop, 0.05);

    sub_a_.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, topic_a_, 1));
    sub_c_.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, topic_c_, 1));
    sync_.reset(new message_filters::Synchronizer<Policy>(Policy(10), *sub_a_, *sub_c_));
    sync_->setMaxIntervalDuration(ros::Duration(slop));
    sync_->registerCallback(boost::bind(&TfMerger::cb, this, _1, _2));

    pub_ = nh.advertise<sensor_msgs::PointCloud2>(out_topic_, 1);
    ROS_INFO_STREAM("pc_tf_merger fixed_frame="<<fixed_frame_<<" A="<<topic_a_<<" C="<<topic_c_);
  }

 private:
  void cb(const sensor_msgs::PointCloud2ConstPtr& a_msg,
          const sensor_msgs::PointCloud2ConstPtr& c_msg) {
    try {
      auto Ta = tf_buffer_.lookupTransform(fixed_frame_, a_msg->header.frame_id, a_msg->header.stamp, ros::Duration(0.05));
      auto Tc = tf_buffer_.lookupTransform(fixed_frame_, c_msg->header.frame_id, c_msg->header.stamp, ros::Duration(0.05));
      Eigen::Matrix4f Ma = tf2::transformToEigen(Ta.transform).matrix().cast<float>();
      Eigen::Matrix4f Mc = tf2::transformToEigen(Tc.transform).matrix().cast<float>();

      CloudRGB::Ptr a(new CloudRGB), c(new CloudRGB), a_tf(new CloudRGB), c_tf(new CloudRGB);
      pcl::fromROSMsg(*a_msg, *a);
      pcl::fromROSMsg(*c_msg, *c);
      pcl::transformPointCloud(*a, *a_tf, Ma);
      pcl::transformPointCloud(*c, *c_tf, Mc);

      CloudRGB::Ptr merged(new CloudRGB);
      merged->reserve(a_tf->size()+c_tf->size());
      *merged += *a_tf; *merged += *c_tf;

      sensor_msgs::PointCloud2 out;
      pcl::toROSMsg(*merged, out);
      out.header.frame_id = fixed_frame_;
      out.header.stamp = std::max(a_msg->header.stamp, c_msg->header.stamp);
      pub_.publish(out);
    } catch (const std::exception& e) {
      ROS_WARN_THROTTLE(1.0, "TF merge skipped: %s", e.what());
    }
  }

  // ROS
  std::string topic_a_, topic_c_, out_topic_, fixed_frame_;
  std::unique_ptr<message_filters::Subscriber<sensor_msgs::PointCloud2>> sub_a_, sub_c_;
  std::unique_ptr<message_filters::Synchronizer<Policy>> sync_;
  ros::Publisher pub_;
  tf2_ros::Buffer tf_buffer_;
  tf2_ros::TransformListener tf_listener_;
};

int main(int argc, char** argv){
  ros::init(argc, argv, "pc_tf_merger");
  ros::NodeHandle nh, pnh("~");
  TfMerger node(nh, pnh);
  ros::spin();
  return 0;
}

